# coding : UTF-8
"""
author：BingBO   time：2023.11.07
Theme：
notes：
"""
import calibrate_camera as cc
import calibrate_laser_plane as cl
import glob
import cv2 as cv


# 对用户提供的接口，输入laser上的图像坐标，转换成相机坐标
def laser_2_CCS(u, v, a, b, c):
    fx, fy, Cx, Cy = cameraMatrix[0][0], cameraMatrix[1][1], cameraMatrix[0][2], cameraMatrix[1][2]
    undistort_point = cv.undistortPoints((u, v), cameraMatrix, distCoeffs, P=cameraMatrix)
    u = undistort_point[0][0][0]
    v = undistort_point[0][0][1]

    # 计算相机坐标
    Yc_coeff = (v - Cy) / fy
    Xc_coeff = (u - Cx) / fx
    Zc = -c / (a * Xc_coeff + b * Yc_coeff - 1)
    Xc = Xc_coeff * Zc
    Yc = Yc_coeff * Zc
    return Xc, Yc, Zc


if __name__ == "__main__":
    # 棋盘格模板规格
    checkerboard_row = 6  # 行 Y (每列点数)
    checkerboard_col = 9  # 列 X (每行点数)
    length = 11.5  # 边长mm
    # 相机标定
    checkerboard_image_path = r"E:\SHU\Research Group\LaserVisionSensor\Calibrate\checkerboard\*.jpg"
    cc.calibrate_camera(checkerboard_image_path, checkerboard_row, checkerboard_col, length)
    # 激光平面标定
    checkerboard_images = glob.glob(checkerboard_image_path)
    checkerboard_images.sort(key=len)  # 排序
    checkerboard_path_list = checkerboard_images[0::3]  # 棋盘格
    laser1_path_list = checkerboard_images[1::3]  # 棋盘格 + laser1
    laser2_path_list = checkerboard_images[2::3]  # 棋盘格 + laser2
    cameraMatrix1, distCoeffs1, R, T, imagePoints = cc.load_calibrate_data()
    laser1_all_points_per_checkerboard = cl.get_intersection_coordinates(laser1_path_list, imagePoints)
    laser2_all_points_per_checkerboard = cl.get_intersection_coordinates(laser2_path_list, imagePoints)

    laser1_all_ccs_points = cl.image_to_camera_coordinates(laser1_all_points_per_checkerboard)
    laser2_all_ccs_points = cl.image_to_camera_coordinates(laser2_all_points_per_checkerboard)
    cl.save_laser_ccs_points_data(laser1_all_ccs_points, laser2_all_ccs_points)
    cl.laser_plane_display()
